// --------------------------------------------------------------------------------------------------------------------
// <copyright file="Ev4FloorFilter.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Vision.Cameras
{
    using System;
    using System.Collections.Generic;
    using System.Drawing;
    using System.Runtime.CompilerServices;
    using System.Runtime.Serialization;
    using Microsoft.Robotics.Core.Algorithms;
    using Microsoft.Robotics.Numerics;

    /// <summary>
    /// Floor filter interface. Instances of this class are expected to analyze depth frames, discover where 
    /// floor plane is, and mark depth frame with estimated floor plane. A soft assumption made in this interface
    /// is that instances of this class are going to be using 2D dynamic fast floor detection as described by
    /// <see cref="http://sharepoint/sites/Baker/_layouts/WordViewer.aspx?id=/sites/Baker/Shared%20Documents/Design%20docs/Dynamic%20Floor%20Extraction.docx"/>
    /// </summary>
    public interface IKinectFloorFilter
    {
        /// <summary>
        /// Set floor filtering data
        /// </summary>
        /// <param name="floorFilteringStripConstants">An array initialized with pre-learned A and B coefficients in rows, per strip of floor (columns)</param>
        void SetCalibrationData(double[][] floorFilteringStripConstants);

        /// <summary>
        /// Walk through entire frame and mark planes (floor, ceiling, etc)
        /// </summary>
        /// <param name="depthFrame">Depth frame</param>
        /// <param name="frameWidth">Frame width</param>
        /// <param name="frameHeight">Frame height</param>
        void FilterPlanesOnFrame(short[] depthFrame, int frameWidth, int frameHeight);

        /// <summary>
        /// Determines whether or not the filter is initialized with pre-learned data
        /// </summary>
        /// <returns>Whether or not learn data has been set</returns>
        bool IsInitialized();

        /// <summary>
        /// This function searches for strips of floor in front of the robot, and computes coefficients applicable to 
        /// future extraction of floor, ceiling, or any other plane parallel to the floor.
        /// </summary>
        /// <param name="depthFrame">Depth frame</param>
        /// <param name="frameWidth">Frame width</param>
        /// <param name="frameHeight">Frame height</param>
        /// <returns>Whether or not floor was learned from the frame</returns>        
        bool TryLearnFloor(short[] depthFrame, int frameWidth, int frameHeight);

        /// <summary>
        /// Applies a linear correction to a depth reading. Right now, this supports 1st and second degree polynomials
        /// It probably makes sense to optimize for one and remove the other once we have enough trust in one of them
        /// </summary>
        /// <param name="depth">Depth reading to convert</param>
        /// <param name="row">The row</param>
        /// <param name="col">The column</param>
        /// <returns>Corrected depth reading</returns>
        short FilterPlanePixels(short depth, int row, int col);

        /// <summary>
        /// Generate coefficients for floor extraction. 
        /// Function we are trying to approximate has a form Y = B / (X - A)
        /// </summary>
        /// <param name="profile">Strip profile</param>
        /// <param name="stripRows">Row count</param>
        /// <param name="startX">Start X</param>
        /// <param name="seedA">best known A</param>
        /// <param name="maxIterations">Max iterations</param>
        /// <param name="smallestAStep">Smallest A step after which search will be stopped</param>
        /// <param name="initialAStep">Initial A step</param>
        /// <param name="outResult">Out result best A and B constants and Variance that we get with those constants against sample data</param>
        /// <returns>Whether or not fitting was at all successful</returns>
        bool FindFittingConstantsForStrip(
            double[] profile,
            int stripRows,
            int startX,
            double seedA,
            int maxIterations,
            double smallestAStep,
            double initialAStep,
            out FastTuple<double, double, double> outResult);

        /// <summary>
        /// Compute strip averages
        /// </summary>
        /// <param name="depthFrame">Depth frame</param>
        /// <param name="frameWidth">Frame width</param>
        /// <param name="startRow">Start row</param>
        /// <param name="endRow">End row</param>
        /// <param name="startCol">Start column</param>
        /// <param name="endCol">End column</param>
        /// <param name="averageStripDepth">Out average strip depth</param>
        /// <returns>Strip average depths</returns>
        double[] GetStripAverages(
            short[] depthFrame,
            int frameWidth,
            int startRow,
            int endRow,
            int startCol,
            int endCol,
            out double averageStripDepth);
    }

    /// <summary>
    /// An instance of IKinectFloorFilter tested on EV4. 
    /// <see cref="http://sharepoint/sites/Baker/_layouts/WordViewer.aspx?id=/sites/Baker/Shared%20Documents/Design%20docs/Dynamic%20Floor%20Extraction.docx"/>
    /// Note. As it stands - this code will not run properly when Kinect has different resolutions during 
    /// calibration and runtime. It should not be a problem as long as we don't intend to change the resolution.
    /// Bug 1495 to track.
    /// </summary>
    public class Ev4FloorFilter : IKinectFloorFilter
    {
        /// <summary>
        /// Portion of the image height to consider during online floor learning 
        /// </summary>
        public const int PortionOfTheFrameHeightToTestForFlatFloor = 10;

        /// <summary>
        /// Determines the maximum iteration count when approximating A. No need to parameterize, since its not something that's
        /// important, necessary, interesting or safe to play with. This is not a magic constant, just a reasonable threshold.
        /// In conjunction with InitialFittingCoefficientSearchStep will support a very wide range of tilt angles
        /// </summary>
        private const int MaxNumberOfApproximationIterations = 100;

        /// <summary>
        /// Determines the precision when approximating A. No need to parameterize, since its not something that's
        /// important, necessary, interesting or safe to play with. This is not a magic constant, just a reasonable threshold.
        /// </summary>
        private const double FloorFittingCoefficientApproximationPrecision = 0.1;

        /// <summary>
        /// Determines the initial A step when approximating value of A. No need to make parameterized, since its not something that's
        /// important, necessary, interesting or safe to play with. This is not a magic constant, just a reasonable threshold.
        /// In conjunction with MaxNumberOfApproximationIterations will support a very wide range of tilt angles. The minus
        /// sign is a performance optimization to start searching down first, does not does not change final result. 
        /// </summary>
        private const int InitialFittingCoefficientSearchStep = -10;

        /// <summary>
        /// Determines the portion of strips that have to have good readings before updating floor profile. I.e. value of 4 would mean
        /// more than 1/4 of the strips must show good readings
        /// </summary>
        private const int MinimumGoodStripRatio = 4;

        /// <summary>
        /// Number of vertical strips used to detect floor
        /// </summary>
        private int stripCount = 0;

        /// <summary>
        /// Floor filtering A constants populated at runtime as we detect the floor
        /// </summary>
        private int[] fittedA = null;

        /// <summary>
        /// Floor filtering B constants populated at runtime as we detect the floor
        /// </summary>
        private int[] fittedB = null;

        /// <summary>
        /// Strip data pruner and validation utility injected at initialization
        /// </summary>
        private IFlatSurfaceLearnDataPruner pruner;

        /// <summary>
        /// Floor cutoff plane threshold constant component
        /// </summary>
        private double thresholdAdjustmentConstantComponent;

        /// <summary>
        /// Floor cutoff plane threshold linear component
        /// </summary>
        private double thresholdAdjustmentLinearComponent;

        /// <summary>
        /// Determines the slope of floor cutoff plane. The higher - the steeper the plane. Depending on how steep the cutoff plane is
        /// we will be able to detect more low level objects at farther distances, albeit at increased risk of false negatives (floor not
        /// marked as such) especially when robot is on challenging terrain such as uneven surfaces, etc.
        /// As a guideline: 0 - results no slope, 0.5 - 1 - is a moderate slope up (0.5 used by default). 2 and more should be considered aggressive, etc
        /// </summary>
        private double cutoffPlaneSlopeAggressiveness;

        /// <summary>
        /// B constant for ceiling extraction. 
        /// Note. For now, we have only 2 planes we extract: floor and the ceiling. As the need to extract multiple
        /// planes parallel to the floor arises (i.e. for multi-level slam maps) - it would make sense to keep an array of B's: one for each 
        /// slice. This will allow very cheap scene slicing.
        /// </summary>
        private int ceilingB = 0;

        /// <summary>
        /// Initializes a new instance of the <see cref="Ev4FloorFilter"/> class.
        /// </summary>
        /// <param name="pruner">Injected pruner</param>
        /// <param name="thresholdAdjustmentConstantComponent">Threshold shift</param>
        /// <param name="thresholdAdjustmentLinearComponent">Threshold linear component</param>
        /// <param name="cutoffPlaneSlopeAggressiveness">Cutoff plane slope aggressiveness factor</param>
        /// <param name="ceilingCutoffInMM">Ceiling cutoff height in MM</param>
        public Ev4FloorFilter(
            IFlatSurfaceLearnDataPruner pruner,
            double thresholdAdjustmentConstantComponent,
            double thresholdAdjustmentLinearComponent,
            double cutoffPlaneSlopeAggressiveness,
            double ceilingCutoffInMM)
        {
            this.pruner = pruner;

            this.thresholdAdjustmentConstantComponent = thresholdAdjustmentConstantComponent;
            this.thresholdAdjustmentLinearComponent = thresholdAdjustmentLinearComponent;
            this.cutoffPlaneSlopeAggressiveness = cutoffPlaneSlopeAggressiveness;

            // 400 is an approximate translation constant from millimeters to B. We want to keep ceiling cutoff resolution-agnostic, so
            // config parameters are kept in mm. Not externalizing this constant as B is resolution-specific at this point. See bug: 1496
            this.ceilingB = (int)(-ceilingCutoffInMM * 400);
        }

        /// <summary>
        /// Walk through entire array and filter our floor 
        /// </summary>
        /// <param name="depthFrame">Depth frame</param>
        /// <param name="frameWidth">Frame width</param>
        /// <param name="frameHeight">Frame height</param>
        public unsafe void FilterPlanesOnFrame(short[] depthFrame, int frameWidth, int frameHeight)
        {
            if (this.IsInitialized())
            {
                this.TryLearnFloor(depthFrame, frameWidth, frameHeight);
            }

            unchecked
            {
                fixed (short* depthImage = depthFrame)
                {
                    for (int row = 0; row < frameHeight; ++row)
                    {
                        int indexRowTemp = row * frameWidth;

                        for (int col = 0; col < frameWidth; ++col)
                        {
                            int pixelIndex = indexRowTemp + col;

                            short depthVal = depthImage[pixelIndex];

                            if (depthVal > 0)
                            {
                                double correctedDepth = this.FilterPlanePixels(depthVal, row, col);

                                depthImage[pixelIndex] = (short)Math.Round(correctedDepth);
                            }
                        }
                    }
                }
            }
        }

        /// <summary>
        /// Initializes an object with floor filtering calibration data
        /// </summary>
        /// <param name="calibrationData">An array initialized with pre-learned A and B coefficients in rows, per strip of floor (columns)</param>
        public void SetCalibrationData(double[][] calibrationData)
        {
            this.stripCount = 0;

            if (null != calibrationData && null != calibrationData[0])
            {
                int stripCount = calibrationData[0].Length;

                if (stripCount < 1)
                {
                    throw new InvalidOperationException("Incorrect number of floor filtering strips. Cannot be less than 1");
                }

                this.stripCount = stripCount;

                this.pruner.SetCalibrationData(calibrationData);
            }
        }

        /// <summary>
        /// Determines whether or not the filter is initialized with pre-learned data
        /// </summary>
        /// <returns>Whether or not learn data has been set</returns>
        public bool IsInitialized()
        {
            return (this.pruner != null) && this.pruner.IsInitialized() && (this.stripCount != 0);
        }

        /// <summary>
        /// Update floor filtering constants. This function searches for strips of floor in front of the robot, and adjusts
        /// A coefficients. Even though it looks like this method is doing a lot of looping - its quite cheap, since it only 
        /// operates on a fraction of the frame
        /// </summary>
        /// <param name="depthFrame">Depth frame</param>
        /// <param name="frameWidth">Frame width</param>
        /// <param name="frameHeight">Frame height</param>
        /// <returns>Whether or not we saw a reliable patch of floor and learned it</returns>
        public bool TryLearnFloor(short[] depthFrame, int frameWidth, int frameHeight)
        {
            int stripHeight = frameHeight / PortionOfTheFrameHeightToTestForFlatFloor;

            // This inedx determines the row of pixels which we will use to fit A and B to (which we do to eliminate any learn instability stability)
            // There is a very high tolerance to what this index can be. It can be the first index in the strip or the last, or anything in between,
            // and results will be pretty good. We are chosing 1/5th from the bottom so that we are close to the bottom of the frame (hence a slightly
            // better chance of a good reading) but not so close that we will be using the very edge pixels which can be somewhat noisy
            int stableRowFromTheBottom = (stripHeight / 5) + 1;

            int startRow = frameHeight - stripHeight;

            int stripWidth = frameWidth / this.stripCount;

            // This means, whatever is the strip count - we take a center half portion of it for online learning. 
            // Too wide - we have less of a chance to find good  floor patch, too narrow - readings will not be as stable
            int floorTestStripWidthInPixels = stripWidth / 2;

            // We compute Average A as seed value. Seeding with good value helps reduce the number of iterations it takes to find 
            // a fitting function, though does not change the end result
            double seedA = frameHeight / 2;

            List<Tuple<double, double>> lastRowdepths = new List<Tuple<double, double>>();

            this.pruner.PrepareForNewPruningSession(frameHeight, frameWidth, this.stripCount);

            for (int i = 0; i < this.stripCount; ++i)
            {
                int startCol = stripWidth * i + stripWidth / 2 - floorTestStripWidthInPixels / 2;
                int endCol = startCol + floorTestStripWidthInPixels;

                double averageStripDepth = 0;
                double[] stripAverages = this.GetStripAverages(
                    depthFrame,
                    frameWidth,
                    startRow,
                    frameHeight,
                    startCol,
                    endCol,
                    out averageStripDepth);

                FastTuple<double, double, double> floorFitResult;
                bool floorFound = this.FindFittingConstantsForStrip(
                    stripAverages,
                    stripHeight,
                    startRow,
                    seedA,
                    MaxNumberOfApproximationIterations,
                    FloorFittingCoefficientApproximationPrecision,
                    InitialFittingCoefficientSearchStep,
                    out floorFitResult);

                int stripCenter = (startCol + endCol) / 2;

                this.pruner.AddSample(stripCenter, floorFitResult, stripAverages[stripHeight - stableRowFromTheBottom]);              
            }

            this.pruner.PruneOutliers();

            bool floorDataIsValid = this.pruner.IsLearnDataValid();

            if (floorDataIsValid)
            {
                List<FastTuple<double, double>> dataPointsA;
                List<FastTuple<double, double>> dataPointsB;

                // Stabilize As, Bs and apply headroom correction. After this, constants are ready to be used for fitting the current scene
                this.PrepareConstantsForFitting(frameHeight, stableRowFromTheBottom, out dataPointsA, out dataPointsB);

                // To have a quick lookup of A and B constants - we pre-compute arrays of them for every width pixel
                this.FitFloorPlaneConstants(frameWidth, frameHeight, dataPointsA, dataPointsB);
            }

            return floorDataIsValid;
        }

        /// <summary>
        /// Applies a linear correction to a depth reading. Right now, this supports 1st and second degree polynomials
        /// It probably makes sense to optimize for one and remove the other once we have enough trust in one of them
        /// </summary>
        /// <param name="depth">Depth reading to convert</param>
        /// <param name="row">The row</param>
        /// <param name="col">The column</param>
        /// <returns>Corrected depth reading</returns>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public unsafe short FilterPlanePixels(short depth, int row, int col)
        {
            // This method works under assumption that floor can be modeled with formula Y = B / (x - A), where
            // x is the row and Y is projected floor plane as seen by camera.
            // We use pre-computed As and Bs and do integer math to minimize perf impact. 
            unchecked
            {
                int floorA;
                int floorB;

                if (null != this.fittedA && null != this.fittedB)
                {
                    floorA = this.fittedA[col];
                    floorB = this.fittedB[col];
                }
                else
                {
                    // If no calibration file is specified, those A and B constants have been emprically found to be good conservative 
                    // values for the floor approximation in EV4 scenarios. No hope of  detecting table legs, etc.  
                    // There is no need to externalize those consts, as they are provided so that EV4s w/o 
                    // floor calibration file dont break right away. Eventually, once we have all floor learn data - we wont need this. 
                    floorA = 200;
                    floorB = 250000;
                }

                // floor can only be detected below horizon pixel
                // ceiling - only above (regardless of where the current horizon is due to tilt/roll)
                if (row > floorA)
                {
                    int floorCutoffDistance = floorB / (row - floorA);

                    if (floorCutoffDistance < depth)
                    {
                        return DepthImageConstants.FloorValue;
                    }
                }
                else if (row < floorA) 
                {
                    // Applying same A for floor and ceiling is a performance optimization, and has the side effect of
                    // having a ceiling cutoff plane slope up by the same degee as the floor cutoff plane does. Its trivial 
                    // to avoid this by preserving unadjusted A when we learn the floor, and then applyng it for ceiling
                    // extraction. Then we'd end up with a ceiling cutoff plane that's perfectly paralel to the floor, as opposed
                    // to being paralel to the sloped up floor cutoff plane. However, fixing this small imperfection would require
                    // one additional A array, a few more lines of code, and a slightly more expensive learning phase. Considering
                    // how small the slope is, we'll fix this bug only if proven neccessary
                    int ceilingCutoffDistance = this.ceilingB / (row - floorA);

                    if (ceilingCutoffDistance < depth)
                    {
                        return DepthImageConstants.CeilingValue;
                    }
                }

                ////// hack. 570 is the height of kinect on EV4 in MM - should be replaced with actual kinect vertical pose, of course
                ////// this can also be significantly sped up by pre-calculating a bunch of stuff here.
                ////int bscalerFactor = (int)floorB / 570;
                ////int height = ((floorB - (depth * (row - floorA))) / bscalerFactor); // this will now get the vertical height of pixel in mm

                ////if (height > 500 || height < 400)
                ////{
                ////    depth = 0; // set everything outside of a slice to 0 (no reading)
                ////}

                return depth;
            }
        }

        /// <summary>
        /// Generate coefficients for floor extraction. 
        /// Function we are trying to approximate has a form Y = B / (X - A)
        /// </summary>
        /// <param name="profile">Strip profile</param>
        /// <param name="stripRows">Row count</param>
        /// <param name="startRowIndexInFrame">Start X</param>
        /// <param name="seedA">best known A</param>
        /// <param name="maxIterations">Max iterations</param>
        /// <param name="smallestAStep">Smallest A step after which search will be stopped</param>
        /// <param name="initialAStep">Initial A step</param>
        /// <param name="outResult">Out result best A and B constants and Variance that we get with those constants against sample data</param>
        /// <returns>Whether or not fitting was at all successful</returns>
        public bool FindFittingConstantsForStrip(
            double[] profile,
            int stripRows,
            int startRowIndexInFrame,
            double seedA,
            int maxIterations,
            double smallestAStep,
            double initialAStep,
            out FastTuple<double, double, double> outResult)
        {
            double currentA = seedA;

            // We do a binary search iteration through different values of A in the direction of minimizing squared error
            double bestVariance = double.MaxValue;

            double currentAStep = initialAStep;
            int iterationCount = 0;

            double bestKnownA = currentA;
            double bestKnownB = 0;

            while (Math.Abs(currentAStep) > smallestAStep)
            {
                FastTuple<double, double, bool> iterationResult =
                    this.FindBAndVarianceForFloorStrip(profile, currentA, stripRows, startRowIndexInFrame);

                if (!iterationResult.Item3)
                {
                    // this means scene does not look like a floor even remotely
                    outResult = new FastTuple<double, double, double>(0, 0, 0);
                    return false;
                }

                if (bestVariance > iterationResult.Item2)
                {
                    bestVariance = iterationResult.Item2;

                    bestKnownA = currentA;
                    bestKnownB = iterationResult.Item1;
                }
                else
                {
                    currentA = bestKnownA;
                    currentAStep = -currentAStep / 2;
                }

                currentA += currentAStep;

                if (++iterationCount > maxIterations)
                {
                    break;
                }
            }

            outResult = new FastTuple<double, double, double>(bestKnownA, bestKnownB, bestVariance);

            return true;
        }

        /// <summary>
        /// Compute strip averages
        /// </summary>
        /// <param name="depthFrame">Depth frame</param>
        /// <param name="frameWidth">Frame width</param>
        /// <param name="startRow">Start row</param>
        /// <param name="endRow">End row</param>
        /// <param name="startCol">Start column</param>
        /// <param name="endCol">End column</param>
        /// <param name="averageStripDepth">Out average strip depth</param>        
        /// <returns>Strip average depths</returns>
        public unsafe double[] GetStripAverages(
            short[] depthFrame,
            int frameWidth,
            int startRow,
            int endRow,
            int startCol,
            int endCol,
            out double averageStripDepth)
        {
            unchecked
            {
                // Bin together pixels for the same row and average them
                int stripHeight = endRow - startRow;

                double[] stripAveragedDepths = new double[stripHeight];

                averageStripDepth = 0;
                int stripGoodReadings = 0;

                fixed (short* depthImage = depthFrame)
                {
                    for (int row = startRow; row < endRow; ++row)
                    {
                        int indexRowTemp = row * frameWidth;

                        int rowIndexInAverageArray = row - startRow;

                        int goodRowReadings = 0;

                        for (int col = startCol; col < endCol; ++col)
                        {
                            int index = indexRowTemp + col;

                            short depth = depthFrame[index];

                            if (depth > 0)
                            {
                                stripAveragedDepths[rowIndexInAverageArray] += depth;
                                ++goodRowReadings;

                                averageStripDepth += depth;
                                ++stripGoodReadings;
                            }
                        }

                        if (goodRowReadings > 0)
                        {
                            stripAveragedDepths[rowIndexInAverageArray] /= goodRowReadings;
                        }
                    }

                    if (stripGoodReadings > 0)
                    {
                        averageStripDepth /= stripGoodReadings;
                    }
                }

                return stripAveragedDepths;
            }
        }

        /// <summary>
        /// Apply linear fitting to entire filtering arrays that will actually be used to filter out floor
        /// </summary>
        /// <param name="frameWidth">Frame width</param>
        /// <param name="frameHeight">Frame height</param>
        /// <param name="dataPointsA">A sample data</param>
        /// <param name="dataPointsB">B sample data</param>
        private void FitFloorPlaneConstants(
            int frameWidth,
            int frameHeight,
            List<FastTuple<double, double>> dataPointsA,
            List<FastTuple<double, double>> dataPointsB)
        {
            PolynomialEstimator solverA = new PolynomialEstimator();
            PolynomialEstimator solverB = new PolynomialEstimator();

            solverA.DataPoints = dataPointsA;
            solverB.DataPoints = dataPointsB;

            FastTuple<double, double> fitA;
            bool fitAResult = solverA.FitALine(out fitA);

            FastTuple<double, double> fitB;
            bool fitBResult = solverB.FitALine(out fitB);

            if (fitAResult && fitBResult)
            {
                if (null == this.fittedA || null == this.fittedB)
                {
                    this.fittedA = new int[frameWidth];
                    this.fittedB = new int[frameWidth];
                }

                for (int i = 0; i < frameWidth; ++i)
                {
                    double interpolatedA = PolynomialEstimator.Estimate(fitA, i);
                    double interpolatedB = PolynomialEstimator.Estimate(fitB, i);

                    this.fittedA[i] = (int)Math.Round(interpolatedA);
                    this.fittedB[i] = (int)Math.Round(interpolatedB);
                }
            }
        }

        /// <summary>
        /// This is where we apply headroom to calculated floor fitting constants to make sure we avoid floor
        /// </summary>
        /// <param name="depthAtPosition">Pruned Depths</param>
        /// <param name="position">Frame height</param>
        /// <param name="clearance">Clearance to apply</param>
        /// <param name="originalA">A constant to adjust</param>
        /// <param name="originalB">B constant To adjust</param>
        private void RecomputeAandBWithClearance(
            double depthAtPosition,
            int position,
            double clearance,
            ref double originalA,
            ref double originalB)
        {
            double headroomAdjustedB =
                ((depthAtPosition + this.cutoffPlaneSlopeAggressiveness * clearance)) * (position - originalA);

            double headroomAdjustedA =
                position - (originalB / (depthAtPosition - (this.cutoffPlaneSlopeAggressiveness + 1) * clearance));

            originalA = headroomAdjustedA;
            originalB = headroomAdjustedB;
        }

        /// <summary>
        /// Re-computes A and B to ensure stable coefficients across the entire width of the frame, and applies headroom adjustment
        /// </summary>
        /// <param name="frameHeight">Frame height</param>
        /// <param name="stableRowFromTheBottom">Row around which to calculate stable As and apply headroom adjustment</param>
        /// <param name="dataPointsA">Original pruned A data points</param>
        /// <param name="dataPointsB">Original pruned B data points</param>
        private void PrepareConstantsForFitting(
            int frameHeight,
            int stableRowFromTheBottom,
            out List<FastTuple<double, double>> dataPointsA,
            out List<FastTuple<double, double>> dataPointsB)
        {
            List<FastTuple<double, double>> dataPointsD;

            this.pruner.GetPrunedDataPoints(out dataPointsA, out dataPointsB, out dataPointsD);

            // Bs could be jumpy when we learned them, and even after pruning - so what we do is we re-compute Bs and As given pruned
            // candidate As and Bs
            double avgB = 0;
            for (int i = 0; i < dataPointsB.Count; ++i)
            {
                avgB += dataPointsB[i].Item2;
            }

            avgB /= dataPointsB.Count;

            int x = frameHeight - stableRowFromTheBottom;

            for (int i = 0; i < dataPointsB.Count; ++i)
            {
                double newA = x - avgB / dataPointsD[i].Item2;
                double newB = avgB;

                double clearance =
                    this.thresholdAdjustmentLinearComponent * dataPointsD[i].Item2 +
                    this.thresholdAdjustmentConstantComponent;

                this.RecomputeAandBWithClearance(
                    dataPointsD[i].Item2,
                    x,
                    clearance,
                    ref newA,
                    ref newB);

                dataPointsA[i] = new FastTuple<double, double>(dataPointsA[i].Item1, newA);
                dataPointsB[i] = new FastTuple<double, double>(dataPointsB[i].Item1, newB);
            }
        }

        /// <summary>
        /// Finds B constant and squared error for a strip of floor recordings given A
        /// </summary>
        /// <param name="stripData">Strip depth array</param>
        /// <param name="initialA">Initial A for which to compute B and the error</param>
        /// <param name="rows">Number of rows</param>
        /// <param name="startX">Start X</param>
        /// <returns>B, variance and whether or not fit was successful</returns>
        private unsafe FastTuple<double, double, bool> FindBAndVarianceForFloorStrip(
            double[] stripData,
            double initialA,
            int rows,
            int startX)
        {
            unchecked
            {
                double sumNumerator = 0;
                double sumDenominator = 0;

                for (int i = 0; i < rows; ++i)
                {
                    double y = stripData[i];
                    int x = i + startX;

                    if (0 != y)
                    {
                        sumNumerator += y / (x - initialA);

                        double denominatorElement = 1 / ((double)x - initialA);
                        sumDenominator += denominatorElement * denominatorElement;
                    }
                }

                if (sumDenominator <= 0)
                {
                    return new FastTuple<double, double, bool>(0, 0, false);
                }

                double b = sumNumerator / sumDenominator; // Resulting B is deterministically computed

                // now that we have both A and B - iterate to find the variance against the sample data
                double squaredError = 0;
                int goodYs = 0;
                for (int i = 0; i < rows; ++i)
                {
                    double y = stripData[i];
                    int x = i + startX;

                    if (0 != y)
                    {
                        double error = y - (b / (x - initialA));
                        squaredError += error * error;
                        ++goodYs;
                    }
                }

                if (goodYs > 0)
                {
                    return new FastTuple<double, double, bool>(b, squaredError / goodYs, true);
                }
                else
                {
                    return new FastTuple<double, double, bool>(0, 0, false);
                }
            }
        }
    }
}
